#include <ros/ros.h>
#include <pluginlib/class_list_macros.h>
#include <geometry_msgs/Twist.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <message_filters/subscriber.h>
#include <image_transport/subscriber_filter.h>
#include <message_filters/time_synchronizer.h>
#include <sensor_msgs/Image.h>
#include <geometry_msgs/Vector3Stamped.h>
#include "opencv2/gpu/gpu.hpp"
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
#include <Eigen/Eigen>
#include <opencv2/opencv.hpp>
#include <message_filters/sync_policies/approximate_time.h>
#include </home/ros/ros_workspace/koala-ambassador/koala_hog/msg_gen/cpp/include/koala_hog/bbox.h>
#include <math.h>
#include </home/ros/ros_workspace/koala-ambassador/koala_kinematics/srv_gen/cpp/include/koala_kinematics/Empty_Int.h>
#include </home/ros/ros_workspace/koala-ambassador/koala_kinematics/srv_gen/cpp/include/koala_kinematics/Int_Bool.h>
#include <ctime>
#include <geometry_msgs/Vector3.h>
#include <std_msgs/Int32.h>
#include <koala_follower/pedestrian.h>

namespace enc = sensor_msgs::image_encodings;
static const char WINDOW[] = "Image window";

ros::ServiceClient drive_distance_client;
ros::ServiceClient turn_angle_client;
ros::Publisher turn_angle_pub;
ros::Publisher drive_distance_pub;
ros::Publisher koala_twist_pub;



float fx = 525.0;
float fy = 525.0;
float cy = 319.5;
float cx = 239.5;
float ds = 1.0;

int colours[6]={0};
bool isTracking = false;
time_t time_last_tracked = time(0);
double last_secs;

ros::Publisher goal_pub;
float last_goal_position_x = 0.0;
float last_goal_position_y = 0.0;

float speed_filter_x = 0.1;
float speed_filter_y = 0.1;

/*
void koala_move(std_msgs::Int32 angle, std_msgs::Int32 distance)
{
    geometry_msgs::Twist koala_drive;
    koala_drive.linear.x = distance.data;
    koala_drive.angular.z = angle.data;
    koala_twist_pub.publish(koala_drive);
}
*/

bool rgb_in_range(int* array)
{
    bool c1 = ( (colours[0]-20) <= array[0] && array[0] <= (colours[0]+20) );
    bool c2 = ( (colours[1]-20) <= array[1] && array[1] <= (colours[1]+20) );
    bool c3 = ( (colours[2]-20) <= array[2] && array[2] <= (colours[2]+20) );
    bool c4 = ( (colours[3]-20) <= array[3] && array[3] <= (colours[3]+20) );
    bool c5 = ( (colours[4]-20) <= array[4] && array[4] <= (colours[4]+20) );
    bool c6 = ( (colours[5]-20) <= array[5] && array[5] <= (colours[5]+20) );

    if(c4&&c5&&c6)
    {
        return true;
    }
    return false;
}


void callback(const sensor_msgs::ImageConstPtr& msg, const koala_hog::bboxConstPtr& bounding_box)
{
    int center_bounding_box_x = bounding_box->x+0.5*bounding_box->width;
    int center_bounding_box_y = bounding_box->y+0.5*bounding_box->height;

    cv_bridge::CvImagePtr cv_ptr;
    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }

    cv::Mat depth_image;
    cv::transpose(cv_ptr->image, depth_image);
    cv::flip(depth_image, depth_image, -1);
    cv::flip(depth_image, depth_image, 1);

    float distance = ds * depth_image.at<float>(center_bounding_box_y, center_bounding_box_x);
    float real_world_x = (center_bounding_box_x - cx) * distance / fx;
    float real_world_y = (center_bounding_box_y - cy) * distance / fy;
    //float area = bounding_box->height * bounding_box->width;


    if(!std::isnan(distance) && !std::isnan(real_world_x) && !std::isnan(real_world_y))// && !real_min_y > 0.4)
    {
        if(real_world_y > -0.2 && real_world_y < 0.4)// && area >= 130000-(30000*(distance-1.8)) && distance < 6.5)
        {
            std::cout << "X: " << real_world_x << ", Y: " << real_world_y << ", Z: " << distance << std::endl;

            // Calculate angle
            float angle = asin(real_world_x/distance) * 57.2957795;
            float travel_distance = distance - 1.5;

            // setup ROS services
            /*
            std_msgs::Int32 pub_angle;
            std_msgs::Int32 pub_distance;

            pub_angle.data = angle * 0.85;
            pub_distance.data = (travel_distance * 100);
            */

            std::cout << "ANGLE: " << angle << std::endl;

            // get RGB values
            int tempRGB[6];
            tempRGB[0] = bounding_box->rl;
            tempRGB[1] = bounding_box->gl;
            tempRGB[2] = bounding_box->bl;
            tempRGB[3] = bounding_box->rt;
            tempRGB[4] = bounding_box->gt;
            tempRGB[5] = bounding_box->bt;


            if(isTracking)
            {
                if(rgb_in_range(tempRGB))
                {
                    std::cout << "IS TRACKING COLOURS OK" <<std::endl;
                    koala_follower::pedestrian goal;
                    goal.position_x = real_world_x;
                    goal.position_y = sqrt((distance*distance) - (real_world_x*real_world_x));
                    float temp_vx = (goal.position_x - last_goal_position_x) / (ros::Time::now().toSec() - last_secs);
                    float temp_vy = (goal.position_y - last_goal_position_y) / (ros::Time::now().toSec() - last_secs);
                    speed_filter_x = speed_filter_x * 0.8 + temp_vx*0.2;  // LEFT / RIGHT
                    speed_filter_y = speed_filter_y * 0.8 + temp_vy*0.2;  // FORWARDS / BACKWARDS
                    goal.velocity_x = speed_filter_x;//(goal.position_x - last_goal_position_x) / (ros::Time::now().toSec() - last_secs);
                    goal.velocity_y = speed_filter_y;//(goal.position_y - last_goal_position_y) / (ros::Time::now().toSec() - last_secs);
                    goal.header.stamp = ros::Time::now();
                    goal.header.frame_id = "odom";

                    goal_pub.publish(goal);
                    // koala_move(pub_angle, pub_distance);
                    for (int i = 0; i < 6; ++i)
                    {
                        colours[i] = tempRGB[i];
                    }
                    last_goal_position_x = goal.position_x;
                    last_goal_position_y = goal.position_y;
                    last_secs = ros::Time::now().toSec();
                }
                else if (ros::Time::now().toSec()-last_secs > 10)
                {
                    std::cout << "WAS TRACKING, BUT AFTER TIMEOUT STARTING TRACKING NEW TARGET" <<std::endl;
                    if(angle <= 5.0 && distance < 4.0)
                    {
                        for (int i = 0; i < 6; ++i)
                        {
                            colours[i] = tempRGB[i];
                        }
                        last_goal_position_x = real_world_x;
                        last_goal_position_y = sqrt((distance*distance) - (real_world_x*real_world_x));
//                      koala_move(pub_angle, pub_distance);
                        last_secs = ros::Time::now().toSec();
                        isTracking = true;
                    }
                    else
                    {
                        std::cout<< "STOPPED TRACKING AFTER TIMEOUT" <<std::endl;
                        last_goal_position_x = 0;
                        last_goal_position_y = 0;
//                      koala_move(pub_angle, pub_distance);
                        isTracking = false;
                    }
                }
                else if(ros::Time::now().toSec()-last_secs < 10)
                {
                    std::cout << "TRYING TRACKING, BUT NO TARGET SEEN BUT NO TIMEOUT YET: " << ros::Time::now().toSec()-last_secs <<std::endl;
                    last_goal_position_x = 0;
                    last_goal_position_y = 0;
//                  koala_move(pub_angle, pub_distance);
                }
                else
                {
                    std::cout<<"TRACKING BUT DOING NOTHING, time " << ros::Time::now().toSec()-last_secs << std::endl;
                    last_goal_position_x = 0;
                    last_goal_position_y = 0;
//                  koala_move(pub_angle, pub_distance);
                }
            }
            else
            {
                if(angle <= 5.0 && distance < 4.0)
                {
                    std::cout<< "TRACKING NEW TARGET" << std::endl;
                    for (int i = 0; i < 6; ++i)
                    {
                        colours[i] = tempRGB[i];
                    }
                    last_goal_position_x = real_world_x;
                    last_goal_position_y = sqrt((distance*distance) - (real_world_x*real_world_x));
//                  koala_move(pub_angle, pub_distance);
                    last_secs = ros::Time::now().toSec();
                    isTracking = true;
                }
            }
        }
    }
}

int main(int argc, char** argv)
{
  cv::namedWindow(WINDOW, CV_WINDOW_NORMAL);

  ros::init(argc, argv, "koala_follower");
  ros::NodeHandle nh;
  image_transport::ImageTransport it(nh);

  last_secs = ros::Time::now().toSec();

  goal_pub = nh.advertise<koala_follower::pedestrian>("/koala/pedestrian", 100);
  drive_distance_pub = nh.advertise<std_msgs::Int32>("/koala/drive_distance_pub", 100);
  turn_angle_pub = nh.advertise<std_msgs::Int32>("/koala/turn_angle_pub", 100);
  koala_twist_pub = nh.advertise<geometry_msgs::Twist>("/koala/twist", 100);


  drive_distance_client = nh.serviceClient<koala_kinematics::Int_Bool>("/koala/drive_distance");
  turn_angle_client = nh.serviceClient<koala_kinematics::Int_Bool>("/koala/turn_angle");

  image_transport::SubscriberFilter image_sub(it, "/camera/depth_registered/image_rect", 1);
  message_filters::Subscriber<koala_hog::bbox> bounding_box_sub(nh, "/hog_bounding_box", 1);


  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, koala_hog::bbox> MySyncPolicy;
  message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(100), image_sub, bounding_box_sub);
  sync.registerCallback(boost::bind(&callback, _1, _2));

  ros::spin();

  return 0;
}
